#include "message_queue.h"
#include <string.h>

/**    
  * @brief  禁用全局中断请求。    
  *         此函数用于暂时屏蔽所有中断请求。    
  * @param  无    
  * @retval	无    
  */  
static void ENTER_CRITICAL(void)
{
    __disable_irq();
}

/**    
  * @brief  启用全局中断请求。    
  *         此函数用于在中断请求被屏蔽后重新启用它们。    
  * @param  无    
  * @retval 无    
  */   
static void EXIT_CRITICAL(void)
{
    __enable_irq();
}

/**
 * @brief 初始化消息队列
 * @param mq        队列对象指针
 * @param buffer    预分配的缓冲区指针
 * @param elem_size 单个元素大小（字节）
 * @param queue_len 队列最大容量（元素数量）
 * @note 建议queue_len设置为2的幂次方以提高计算效率
 */
void mq_init(message_queue_t *mq, void *buffer, 
            uint16_t elem_size, uint16_t queue_len)
{
    mq->buffer = (uint8_t*)buffer;
    mq->elem_size = elem_size;
    mq->queue_len = queue_len;
    mq->head = 0;
    mq->tail = 0;
}

/**
 * @brief 写入单个元素（非阻塞）
 * @param mq    队列对象指针
 * @param data  待写入数据指针
 * @return true 写入成功
 * @return false 队列已满，写入失败
 */
bool mq_send(message_queue_t *mq, const void *data)
{
    ENTER_CRITICAL();
    bool ret = false;
    
    if(!mq_is_full(mq)) {
        uint8_t *target = &mq->buffer[mq->tail * mq->elem_size];
        memcpy(target, data, mq->elem_size);
        mq->tail = (mq->tail + 1) % mq->queue_len;
        ret = true;
    }
    
    EXIT_CRITICAL();
    return ret;
}

bool mq_receive(message_queue_t *mq, void *data)
{
    bool ret = false;
    ENTER_CRITICAL();
    
    if(mq->head != mq->tail) {
        uint8_t *source = &mq->buffer[mq->head * mq->elem_size];
        memcpy(data, source, mq->elem_size);
        mq->head = (mq->head + 1) % mq->queue_len;
        ret = true;
    }
    
    EXIT_CRITICAL();
    return ret;
}

uint16_t mq_get_free_space(message_queue_t *mq)
{
    uint16_t free;
    ENTER_CRITICAL();
    free = (mq->queue_len + mq->head - mq->tail - 1) % mq->queue_len;
    EXIT_CRITICAL();
    return free;
}


uint16_t mq_get_count(message_queue_t *mq)
{
    uint16_t count;
    ENTER_CRITICAL();
    count = (mq->queue_len + mq->tail - mq->head) % mq->queue_len;
    EXIT_CRITICAL();
    return count;
}

/**
 * @brief 批量写入元素（非阻塞）
 * @param mq        队列对象指针
 * @param data      数据源指针
 * @param num_elems 请求写入的元素数量
 * @return 实际写入的元素数量
 */
uint16_t mq_send_bulk(message_queue_t *mq, const void *data, uint16_t num_elems)
{
    ENTER_CRITICAL();
    
    uint16_t free_space = mq_get_free_space(mq);
    uint16_t actual_write = (num_elems < free_space) ? num_elems : free_space;
    
    if(actual_write > 0) {
        uint16_t available_contiguous = mq->queue_len - mq->tail;
        
        // 分两次拷贝（当尾部空间不足时）
        uint16_t first_copy = (actual_write <= available_contiguous) ? 
                             actual_write : available_contiguous;
        uint16_t second_copy = actual_write - first_copy;
        
        memcpy(&mq->buffer[mq->tail * mq->elem_size], 
              data, 
              first_copy * mq->elem_size);
        
        if(second_copy > 0) {
            memcpy(mq->buffer, 
                  (uint8_t*)data + first_copy * mq->elem_size, 
                  second_copy * mq->elem_size);
        }
        
        mq->tail = (mq->tail + actual_write) % mq->queue_len;
    }
    
    EXIT_CRITICAL();
    return actual_write;
}

/**
 * @brief 批量读取元素（非阻塞）
 * @param mq        队列对象指针
 * @param data      目标缓冲区指针
 * @param num_elems 请求读取的元素数量
 * @return 实际读取的元素数量
 */
uint16_t mq_receive_bulk(message_queue_t *mq, void *data, uint16_t num_elems)
{
    ENTER_CRITICAL();
    
    uint16_t available = mq_get_count(mq);
    uint16_t actual_read = (num_elems < available) ? num_elems : available;
    
    if(actual_read > 0) {
        uint16_t available_contiguous = mq->queue_len - mq->head;
        
        // 分两次拷贝（当头部连续空间不足时）
        uint16_t first_copy = (actual_read <= available_contiguous) ? 
                             actual_read : available_contiguous;
        uint16_t second_copy = actual_read - first_copy;
        
        memcpy(data, 
              &mq->buffer[mq->head * mq->elem_size], 
              first_copy * mq->elem_size);
        
        if(second_copy > 0) {
            memcpy((uint8_t*)data + first_copy * mq->elem_size, 
                  mq->buffer, 
                  second_copy * mq->elem_size);
        }
        
        mq->head = (mq->head + actual_read) % mq->queue_len;
    }
    
    EXIT_CRITICAL();
    return actual_read;
}

/* 队列清除 */
void mq_clear(message_queue_t *mq)
{
    ENTER_CRITICAL();
    mq->head = 0;
    mq->tail = 0;
    EXIT_CRITICAL();
}

bool mq_is_empty(message_queue_t *mq) {
    return (mq->head == mq->tail);
}

bool mq_is_full(message_queue_t *mq) {
    return ((mq->tail + 1) % mq->queue_len) == mq->head;
}

